#!/usr/bin/env python
#!coding=utf-8

#right code !
#write by leo at 2018.04.26
#function: 
#display the frame from another node.

import rospy
import numpy as np
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
import cv2
from matplotlib import pyplot as plt

def biggernum(a,b):
    if a>b:
        return b,a
    else:
        return a,b

def cut_diatance(cv_img):
    ball_color = 'yellow'

    color_dist = {'red': {'Lower': np.array([0, 60, 60]), 'Upper': np.array([6, 255, 255])},
                  'blue': {'Lower': np.array([100, 80, 46]), 'Upper': np.array([124, 255, 255])},
                  'green': {'Lower': np.array([35, 43, 35]), 'Upper': np.array([90, 255, 255])},
                  'yellow':{'Lower':np.array([0,10,10]),'Upper':np.array([100,255,255])}
                  }

    frame = cv_img
#    frame = cv2.flip(frame, 1)
    frame = cv2.flip(frame, 0)

    if frame is not None:
        gs_frame = cv2.GaussianBlur(frame, (5, 5), 0)                     # ��~X�~V�模��~J
        hsv = cv2.cvtColor(gs_frame, cv2.COLOR_BGR2HSV)                 # 转�~L~V�~H~PHSV�~[��~C~O
        erode_hsv = cv2.erode(hsv, None, iterations=2)                   # �~E~P�~Z~@ ��~W�~Z~D�~O~X��~F
        inRange_hsv = cv2.inRange(erode_hsv, color_dist[ball_color]['Lower'], color_dist[ball_color]['Upper'])
        cnts = cv2.findContours(inRange_hsv.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]

        c = max(cnts, key=cv2.contourArea)
        rect = cv2.minAreaRect(c)
        box = cv2.boxPoints(rect)
        cv2.drawContours(frame, [np.int0(box)], -1, (0, 255, 255), 2)

    #��~B�~[~R��~P中��~C�~Z~D��~^�~Y~E��~]离
        core_point = rect[0]
        core_point_x = core_point[0]
        core_point_y = core_point[1]
        Height_Width = rect[1]
        Height = Height_Width[1]
        Width = Height_Width[0]
        PointDistance_x = core_point_x - 320
        PointDistance_y = core_point_y - 240
        RealDistance_x = - PointDistance_x * 9.5 / Width * 10
        
        RealDistance_y = PointDistance_y * 9.5 / Height * 10 

        f = open('./Distance.txt','w')
        f.write(str(RealDistance_x))
        f.write('\n')
        f.write(str(RealDistance_y))
        f.close

    #��~A�~I��~[��~I~G    
        img = []
        img = frame
        box1 = box[1]
        box2 = box[2]
        box3 = box[3]
        box4 = box[0]
        a_point_x1 = box1[0]
        b_point_x1 = box2[0]
        c_point_x1 = box3[0]
        d_point_x1 = box4[0]
        a_point_y1 = box1[1]
        b_point_y1 = box2[1]
        c_point_y1 = box3[1]
        d_point_y1 = box4[1]
        central_point_x1 = (a_point_x1 + c_point_x1) * 0.5
        central_point_y1 = (a_point_y1 + c_point_y1) * 0.5

        x = int((c_point_x1 - a_point_x1))
        y = int((c_point_y1 - a_point_y1))

        x1 = central_point_x1 - a_point_x1
        y1 = central_point_y1 - b_point_y1
        S = x * y
        if S != 0 :
            if (central_point_y1 - y1 < 0) or (central_point_x1 - x1 < 0):
                img1 = img[0:int(central_point_y1 + y1),0:int(central_point_x1 + x1)]
            else:
                a1 = int(central_point_y1 - y1)
                a2 = int(central_point_y1 + y1)
                b1 = int(central_point_x1 - x1)
                b2 = int(central_point_x1 + x1)
                a1, a2 = biggernum(a1, a2)
                b1, b2 = biggernum(b1, b2)
                img = img[a1:a2,b1:b2]
        return img, frame

def displayUSBcam():
    i = 0
    print(i)
    rospy.init_node('usb_cam_display', anonymous=True)
    # make a video_object and init the video object
    global count,bridge
    scaling_factor = 0.5
    bridge = CvBridge()
    data = rospy.wait_for_message("/usb_cam/image_raw", Image, timeout=None)
    count = count + 1
    cv_img = bridge.imgmsg_to_cv2(data, "bgr8")
    cv_img, frame = cut_diatance(cv_img)
    cv2.imshow("camera", frame)
    cv2.imshow("frame" , cv_img)
    cv2.imwrite("/home/zhangyuting/eaibot/PaddleOCR-release-1.1/doc/imgs/province.jpg", cv_img)
    cv2.waitKey(3)

if __name__ == '__main__':
    global count,bridge
    count = 0
    while 1:
        displayUSBcam()



